#include <stdio.h>
#include <math.h>

#define FOSC 40000000
#define TIMERPERIOD (double)(4*(1000000000.0 / FOSC))

/* Linear */
double f(double t) {
  return t;
}

/* Sigmoid */
/*
double f(double t) {
  double ef=10; // shape of the sigmoid, larger ef makes it sharper
  return (1 / (1 + exp(-(ef*t - (ef/2)))));
}
*/

double v(double timet, double vi, double vf, double total ) {
  //printf("v(%g %g %g %g = %g\n", timet, vi, vf, total, (vi + (vf - vi) * f(timet/total)));
  return (vi + (vf - vi) * f(timet/total));
}

int main(void) {
  unsigned int i;
  double a, j, vcomp, vfinal;
  double  step360;
  double t=0.0, dt=0.0, vang, vinitial;
  double timer, timerinitial, timerfinal, timerstep, timerperiod, timertotal, totaltime;
  double timercomp, alpha, C, TP;
  unsigned int nbpas=0, totalpas;

  totaltime=2.000000000;
  totalpas=0;

  step360 =400;
  timerinitial = 65535;
  timerstep = timerinitial;
  timertotal = 0.0;
  timerfinal=6250;
  vfinal=(360.0 / step360) / ( (double)timerfinal * TIMERPERIOD / 1000000000.0); 
  vinitial=(360.0 / step360) / ( (double)timerinitial * TIMERPERIOD / 1000000000.0); 

  timercomp=timerinitial;
  C=(360.0/step360);
  alpha = (vfinal - vinitial) / totaltime;
  TP = TIMERPERIOD / 1000000000.0;

  //vfinal=1440.0;
  i=0;
  if (totalpas == 0) {
    //nbinc = 50;
    //nbacc = 200;
  } else {
    unsigned int timermean;
    timermean= ((360.0 / step360) / ((vfinal-vinitial)/2)) * (1000000000.0 / TIMERPERIOD);
    totaltime = timermean *totalpas *TIMERPERIOD;
    //nbacc=totalpas/6;
    //nbinc=(nbacc/4);
  }

  //timerperiod=(totaltime/nbacc) / TIMERPERIOD;
  timer=timerinitial;
  //t = t + ((double)timer * TIMERPERIOD / 1000000000.0);
  vang=(360.0 / step360) / ( (double)timer * TIMERPERIOD / 1000000000.0); 
  a=0; //j=(vfinal - v) / ((nbacc - nbinc + 1) * nbinc);
  //printf("vfinal = %g vinit = %g j = %g timer %g totaltime %g timerperiod %g\n", vfinal, v, j, timer, totaltime, timerperiod);
  while (timer > timerfinal  + 10) {
    double dt=timer;
    nbpas+=1;
    //timerstep+=timer;
    /*if (timerstep >= timerperiod) {
      double dti=timer, l=0.;
      if (i < nbinc) a += j;
      if (i > (nbacc - nbinc)) a -= j;
      v = v + a;
      timer = ((360.0 / step360) / v) * (1000000000.0 / TIMERPERIOD);
      dti = dti - timer;
      l = dti / dt;
      dt = dti;
      timerstep=timer;
      i+=1;
      //printf("time %g -- step %d: acc. step %d, timer %g dt %g v %g a %g d2t %g\n", t, nbpas, i, timer, dt, v, a, l);
      printf("%g %g %g %g %g %d %d\n", t, timer, v, dt, l, i, nbpas);
      //printf("%d %g\n", nbpas, timer);
      //printf("%d %g\n", nbpas, timer);
      //printf("%g %g %d\n", t, dt, nbpas);
    }
    */
    t = t + ((double)timer * TIMERPERIOD / 1000000000.0);
    vcomp=v(t, vinitial, vfinal, totaltime);
    //printf("%g %g %g -- ", t, vcomp, v(t, vinitial, vfinal, totaltime));
    timer = (360.0 / step360) / (v(t, vinitial, vfinal, totaltime) * (TIMERPERIOD / 1000000000.0));
    vang=(360.0 / step360) / ( (double)timer * TIMERPERIOD / 1000000000.0); 
    /*    printf("%g %g %g %d\n", t,  timer, vang, i);*/
    timercomp= (C * timercomp) / (C + (alpha * (TP*TP) * (timercomp * timercomp)));
    dt -= timer;
    if (dt > 400) dt =0.0;
    //timercomp= timercomp - ((alpha * TP / C) * (timercomp * timercomp));
    printf("%g %g %g %g %g %g %d\n", t, timer, timercomp, dt, vcomp, vang, nbpas);
    //printf("%g %g\n", t, timer);
  }

  exit(0);
}
